#include <aversive.h>
#include <aversive/wait.h>
#include <uart.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "i2cm.h"
#include "compass.h"
#include "comm_fpga.h"

#include <avr/sfr_defs.h>

// 
int main(void)
{
  // Turn interruptions ON
  sei();

  // Initialize UART
  uart_init();
  fdevopen(uart1_dev_send, uart1_dev_recv);

  // Some advertisment :p
  printf("\n\nRobotter 2008 - 102chute - UNIOC MOTORS STRESS TESTS\n");
  printf("Compiled "__DATE__" at "__TIME__"\n");
  printf("\nBooting...\n");

  // Initialize compass
  compass_init();

  // Initialize FPGA communication
  commfpga_init();

  // Remove parking break
  sbi(DDRB,7);
  sbi(PORTB,7);

  // Ready up
  printf("done.\n\n");
  
  double d=0.0;
  double t=0.0;
  int32_t m1,m2,m3;

  while(1)
  {
    t+=0.1;
    m1 = 1000*sin(t);
    m2 = 1000*sin(t+d*(M_PI/3));
    m3 = 1000*sin(t+d*(2*M_PI/3));

    commfpga_set_M1(NULL, m1);
    commfpga_set_M2(NULL, m2);
    commfpga_set_M3(NULL, m3);

    printf("\rm1 = %5ld m2 = %5ld m3 = %5ld bearing = %5d        ",m1,m2,m3,compass_getBearing_deg());
    wait_ms(50);
  }

  while(1) nop();
  return 0;
}
